/**
 * @file bounding_box.cpp
 * @author tangjf (ryontang@163.com)
 * @brief 
 * @version 0.1
 * @date 2020-12-07
 * 
 * @copyright Copyright (c) 2020
 * 
 */

// include personal lib.
//#include "bounding_box.h"
#include "n_lidar_obj/bounding_box/bounding_box.h"

// include system lib.
#include <cmath>
//#include <opencv2/core/core.hpp>
//#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/imgproc/imgproc.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <visualization_msgs/Marker.h>

#define EIGEN_MPL2_ONLY

#include <Eigen/Core>

namespace lidar
{
namespace obj
{

bool BoundingBoxModel::estimate(
    const msg_lidar_prep::msg_lidar_prep &cluster,
    msg_lidar_shape::DetectedObject &output)
{
  //  std::cout << "calling BoundingBoxModel::estimate" << std::endl;
  // calc centroid point for bounding_box height(z)
  msg_lidar_prep::LidarPoints centroid;
  centroid.x = 0;
  centroid.y = 0;
  centroid.z = 0;
  for (const auto &lidar_point : cluster.lidar_prep)
  {
    centroid.x += lidar_point.x;
    centroid.y += lidar_point.y;
    centroid.z += lidar_point.z;
  }

  // Note that centroid.z isn't center.z,centroid.z
  // is height at box,not plane.
  centroid.x = centroid.x / (double)cluster.lidar_prep.size();
  centroid.y = centroid.y / (double)cluster.lidar_prep.size();
  centroid.z = centroid.z / (double)cluster.lidar_prep.size();

  // calc min and max z for bounding_box length
  double min_z = 0;
  double max_z = 0;
  double min_x = 0;
  double max_x = 0;
  double min_y = 0;
  double max_y = 0;
  for (size_t i = 0; i < cluster.lidar_prep.size(); ++i)
  {
    if (cluster.lidar_prep.at(i).z < min_z || i == 0) //点云簇第i个点的z值
      min_z = cluster.lidar_prep.at(i).z;
    if (max_z < cluster.lidar_prep.at(i).z || i == 0)
      max_z = cluster.lidar_prep.at(i).z;

    if (cluster.lidar_prep.at(i).x < min_x || i == 0) //点云簇第i个点的z值
      min_x = cluster.lidar_prep.at(i).x;
    if (max_x < cluster.lidar_prep.at(i).x || i == 0)
      max_x = cluster.lidar_prep.at(i).x;

    if (cluster.lidar_prep.at(i).y < min_y || i == 0) //点云簇第i个点的z值
      min_y = cluster.lidar_prep.at(i).y;
    if (max_y < cluster.lidar_prep.at(i).y || i == 0)
      max_y = cluster.lidar_prep.at(i).y;
  }

  // tjf revise
  double delta_x = max_x - min_x;
  double delta_y = max_y - min_y;
  double delta_z = max_z - min_z;

  // 滤掉大的障碍物
  if (delta_z > 5.0 || (delta_y > 5.0 && delta_x > 5.0) || delta_y > 12.0 || delta_x > 12.0)
  {
    return false;
  }
  /*// calc circumscribed circle on x-y plane
  cv::Mat_<float> cv_points((int)cluster.lidar_prep.size(), 2);
  for (size_t i = 0; i < cluster.lidar_prep.size(); ++i)
  {
    cv_points(i, 0) = cluster.lidar_prep.at(i).x;  // x.
    cv_points(i, 1) = cluster.lidar_prep.at(i).y;  // y
  }*/

  // Paper : Algo.2 Search-Based Rectangle Fitting
  std::vector<std::pair<double /*theta*/, double /*q*/>>
      q_vector;
  const double max_angle = M_PI / 2.0;
  // const double angle_reso = M_PI / 180.0;
  // 步长改为
  const double angle_reso = M_PI / 90.0;
  double theta = atan2(delta_y, delta_x);
  for (/*double theta = 0*/; theta < max_angle; theta += angle_reso)
  {
    Eigen::Vector2d e_1;
    e_1 << std::cos(theta), std::sin(theta); // col.3, Algo.2
    Eigen::Vector2d e_2;
    e_2 << -std::sin(theta), std::cos(theta); // col.4, Algo.2
    std::vector<double> C_1;                  // col.5, Algo.2
    std::vector<double> C_2;                  // col.6, Algo.2
    for (const auto &point : cluster.lidar_prep)
    {
      C_1.push_back(point.x * e_1.x() + point.y * e_1.y());
      C_2.push_back(point.x * e_2.x() + point.y * e_2.y());
    }
    // 找q_vector中最小的，找一个最佳theta
    double q = CalcClosenessCriterion(C_1, C_2);  // col.7, Algo.2
    q_vector.push_back(std::make_pair(theta, q)); // col.8, Algo.2
  }

  double theta_star; // col.10, Algo.2
  double max_q;
  for (size_t i = 0; i < q_vector.size(); ++i)
  {
    // max_q isn't given initial value,but when
    // i==0,Q.at(0).second is max_q.
    if (max_q < q_vector.at(i).second || i == 0)
    {
      max_q = q_vector.at(i).second;

      // 找到最佳的theta_star了
      theta_star = q_vector.at(i).first;
    }
  }

  // col.11, Algo.2
  Eigen::Vector2d e_1_star;
  Eigen::Vector2d e_2_star;
  e_1_star << std::cos(theta_star), std::sin(theta_star);
  e_2_star << -std::sin(theta_star), std::cos(theta_star);

  // col.11, Algo.2
  std::vector<double> C_1_star;
  // col.11, Algo.2
  std::vector<double> C_2_star;
  // 利用最佳theta_star重新算一遍从 c1 c2
  for (const auto &point : cluster.lidar_prep)
  {
    // col.11, Algo.2
    C_1_star.push_back(point.x * e_1_star.x() + point.y * e_1_star.y());
    // col.11, Algo.2
    C_2_star.push_back(point.x * e_2_star.x() + point.y * e_2_star.y());
  }

  // col.12, Algo.2
  const double min_C_1_star =
      *std::min_element(C_1_star.begin(), C_1_star.end());
  const double max_C_1_star =
      *std::max_element(C_1_star.begin(), C_1_star.end());
  const double min_C_2_star =
      *std::min_element(C_2_star.begin(), C_2_star.end());
  const double max_C_2_star =
      *std::max_element(C_2_star.begin(), C_2_star.end());

  // rectangle edges is 4 sides expression: a[i]*x + b[i]*y = c[i], i=1,2,3,4
  const double a_1 = std::cos(theta_star);
  const double b_1 = std::sin(theta_star);
  const double c_1 = min_C_1_star;
  const double a_2 = -1.0 * std::sin(theta_star);
  const double b_2 = std::cos(theta_star);
  const double c_2 = min_C_2_star;
  const double a_3 = std::cos(theta_star);
  const double b_3 = std::sin(theta_star);
  const double c_3 = max_C_1_star;
  const double a_4 = -1.0 * std::sin(theta_star);
  const double b_4 = std::cos(theta_star);
  const double c_4 = max_C_2_star;

  // calc center of bounding box 即矩形两条垂直边的交点
  // edge 1 & edge 2 intersection point
  double intersection_x_1 = (b_1 * c_2 - b_2 * c_1) / (a_2 * b_1 - a_1 * b_2);
  double intersection_y_1 = (a_1 * c_2 - a_2 * c_1) / (a_1 * b_2 - a_2 * b_1);
  // edge 3 & edge 4 intersection point
  double intersection_x_2 = (b_3 * c_4 - b_4 * c_3) / (a_4 * b_3 - a_3 * b_4);
  double intersection_y_2 = (a_3 * c_4 - a_4 * c_3) / (a_3 * b_4 - a_4 * b_3);
  // edge 1 & edge 4 intersection point
  double intersection_x_1_4 = (b_1 * c_4 - b_4 * c_1) / (a_4 * b_1 - a_1 * b_4);
  double intersection_y_1_4 = (a_1 * c_4 - a_4 * c_1) / (a_1 * b_4 - a_4 * b_1);
  // edge 2 & edge 3 intersection point
  double intersection_x_2_3 = (b_2 * c_3 - b_3 * c_2) / (a_3 * b_2 - a_2 * b_3);
  double intersection_y_2_3 = (a_2 * c_3 - a_3 * c_2) / (a_2 * b_3 - a_3 * b_2);

  // 求质心
  double x_center = (intersection_x_1 + intersection_x_2 + intersection_x_1_4 +
                     intersection_x_2_3) /
                    4.0;
  double y_center = (intersection_y_1 + intersection_y_2 + intersection_y_1_4 +
                     intersection_y_2_3) /
                    4.0;
  double z_center = centroid.z;

  // calculate rectangle edge intersection for lines markerArray
  // 矩形边缘交点有四个
  geometry_msgs::Point intersect_1_2, intersect_3_4, intersect_1_4,
      intersect_2_3;
  intersect_1_2.x = intersection_x_1;
  intersect_1_2.y = intersection_y_1;
  intersect_1_2.z = centroid.z;
  intersect_3_4.x = intersection_x_2;
  intersect_3_4.y = intersection_y_2;
  intersect_3_4.z = centroid.z;
  intersect_1_4.x = intersection_x_1_4;
  intersect_1_4.y = intersection_y_1_4;
  intersect_1_4.z = centroid.z;
  intersect_2_3.x = intersection_x_2_3;
  intersect_2_3.y = intersection_y_2_3;
  intersect_2_3.z = centroid.z;

  // 使用marker的LINE_LIST类型可视化
  visualization_msgs::Marker line_list;
  line_list.header.frame_id = "/base_link";
  line_list.header.stamp = ros::Time::now();
  line_list.ns = "L-shape_lines";
  line_list.action = visualization_msgs::Marker::ADD;
  line_list.pose.orientation.w = 1.0;
  line_list.id = 2;
  line_list.type = visualization_msgs::Marker::LINE_LIST;
  line_list.scale.x = 0.1;
  line_list.color.r = 1.0;
  line_list.color.a = 1.0;
  //  line_list.points.push_back(intersect_1_2);
  //  line_list.points.push_back(intersect_2_3);
  //  pub_line.publish(line_list);
  output.line_list.points.push_back(intersect_1_2);
  output.line_list.points.push_back(intersect_2_3);

  // calc dimention/dimension of bounding box
  Eigen::Vector2d e_x;
  Eigen::Vector2d e_y;
  e_x << a_1 / (std::sqrt(a_1 * a_1 + b_1 * b_1)),
      b_1 / (std::sqrt(a_1 * a_1 + b_1 * b_1));
  e_y << a_2 / (std::sqrt(a_2 * a_2 + b_2 * b_2)),
      b_2 / (std::sqrt(a_2 * a_2 + b_2 * b_2));
  Eigen::Vector2d diagonal_vec;
  // intersection_x_1 - intersection_x_2 may be negetive.so at
  // output.dimensions.x call fabs.
  diagonal_vec << intersection_x_1 - intersection_x_2,
      intersection_y_1 - intersection_y_2;

  // calc yaw 只要算航向角所以前面两项为0
  tf2::Quaternion quat;
  quat.setEuler(/* roll */ 0, /* pitch */ 0,
                /* yaw */ std::atan2(e_1_star.y(), e_1_star.x()));

  /********************************************************************************************/
  /************Here return object's position x,y,z and orientation
   *yaw.************************/
  /********************************************************************************************/
  //  std::cout
  //    << "*******************here is calcucating x,y,z,yaw****************"
  //    << std::endl;

  output.pose.position.x = (intersection_x_1 + intersection_x_2) / 2.0;
  //  std::cout << "position.x is :" << output.pose.position.x << std::endl;

  output.pose.position.y = (intersection_y_1 + intersection_y_2) / 2.0;
  //  std::cout << "position.y is :" << output.pose.position.y << std::endl;

  output.pose.position.z = centroid.z;
  //  std::cout << "position.z is :" << output.pose.position.z << std::endl;

  // 给航向角
  output.pose.orientation = tf2::toMsg(quat);
  //  std::cout << "orientation is :" << output.pose.orientation << std::endl;

  output.angle = std::atan2(e_1_star.y(), e_1_star.x());
  //  std::cout << "the yaw angle is :" << output.angle << std::endl;

  constexpr double ep = 0.001;
  // e_x.dot(diagonal_vec): both vector are e_x & diagonal_vec. dot product.
  //(a, b).dot((c, d)) = a*c + b*d. here difined the dimension of box.
  // 给长宽高及其他信息
  output.dimensions.x = std::fabs(e_x.dot(diagonal_vec));
  output.dimensions.y = std::fabs(e_y.dot(diagonal_vec));
  output.dimensions.z = std::max((max_z - min_z), ep);
  output.pose_reliable = true;
  //  std::cout << "output.dimensions is :" << output.dimensions << std::endl;

  // check wrong output
  if (output.dimensions.x < ep && output.dimensions.y < ep)
    return false;
  output.dimensions.x = std::max(output.dimensions.x, ep);
  output.dimensions.y = std::max(output.dimensions.y, ep);
  output.valid = true;
  output.pose_reliable = true;
  output.label = "truck";
  return true;
}

double BoundingBoxModel::CalcClosenessCriterion(
    const std::vector<double> &C_1, const std::vector<double> &C_2)
{
  // Paper : Algo.4 Closeness Criterion
  const double min_c_1 =
      *std::min_element(C_1.begin(), C_1.end()); // col.2, Algo.4
  const double max_c_1 =
      *std::max_element(C_1.begin(), C_1.end()); // col.2, Algo.4
  const double min_c_2 =
      *std::min_element(C_2.begin(), C_2.end()); // col.3, Algo.4
  const double max_c_2 =
      *std::max_element(C_2.begin(), C_2.end()); // col.3, Algo.4

  std::vector<double> D_1; // col.4, Algo.4
  for (const auto &c_1_element : C_1)
  {
    const double v = std::min(max_c_1 - c_1_element, c_1_element - min_c_1);
    D_1.push_back(std::fabs(v));
  }

  std::vector<double> D_2; // col.5, Algo.4
  for (const auto &c_2_element : C_2)
  {
    const double v = std::min(max_c_2 - c_2_element, c_2_element - min_c_2);
    D_2.push_back(v * v);
  }

  const double d_min = 0.05;
  const double d_max = 0.50;
  double beta = 0; // col.6, Algo.4
  for (size_t i = 0; i < D_1.size(); ++i)
  {
    const double d =
        std::min(std::max(std::min(D_1.at(i), D_2.at(i)), d_min), d_max);
    beta += 1.0 / d;
  }
  return beta;
}
} // namespace obj
} // namespace lidar
